A Reproduced Copy 

OF 



Reproduced for NASA 
by the 

NASA Scientific and Technical Information Facility 


i 

i 

s 


natioK^Schnical 1 
INfSffloN SERVICE . 

SpringBal<i'_V5.' — 


FFNo 672 Aug 65 



TE-43 


COMPUTATIOKMi COMPARISON OF -STRAPDOWN SYSTEM 
ATTITUDE ALGORITHMS 

by 

Fred J. Marcus 
June 1371 


Approved : 

Director - [ 

Keasureiftent Systems Laboratory 


Massachusetts Institute of Technology 
Measurement Systems Laboratory 
Cambridge, Massachusetts 02133 


' -O . 




TE-43 


COMPUTATIONAL COMPARISON OF STRAPDOWK SYSTEM 
ATTITUDE ALGORITHMS 

i^y. 


Fred J. Marcus 
B.S. U96B) 

Massachusetts Institute of Technology 


SUBMITTED IN PARTIAL FULPILLi-IENT 
OF THE REQUIREMENTS FOR THE 
DEGREE OF MASTER OF 
SCIENCE 


at the 

-MASSACHUSETTS INSTITUTE OF TECHNOLOGY 
June 1971 


Signature of Author 



Department of Aeronautics and Astronautics, 
June 1971 


Certified by 


Accepted by 


: ^ ^ -j— 

Thesis Supervisor ! 




The accuracies of two strapdovm system attitude algorithms are 
compared by digital computer simulation of the algorithms' response 
to specified angular input rates. For constant rate cases, both the 
direction cosine matrix (D.C.M.) technique and the Euler parameter 
technique gave rise to linearly growing drift angle errors. The 
errors incurred by the Euler parameter technique v;ere significantly 
less than those incurred by the direction cosine matrix scheme. 

For the cases where the simulated body motions were purely 
sinusoidal, the drift errors incurred by both techniques were bounded 
sinusoids having the same periods as the applied angular inputs . The 
magnitudes of the bounds were again less for those routines using 
Euler parameters rather than D.C.M, schemes. The magnitudes of the 
bounds for this case v;ere also found to be proportional to the input 
angular rate raised to the power of the order of the numerical 
integration scheme employed. 
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CHAPTER 1. 

Introduction 

Strapdown navigation systems enable the determination of vehicle 
motions, position, and attitude with respect to some reference frame 
by using gyros and accelerometers t^hich are fixed to the vehicle. 
Because these instruments are fixed to the vehicle, they are always in 
the Scune orientation v?ith respect to any coordinate frame also fixed 
in the vehicle. This hypothetical frame fixed in the body is called 
the body frame . 

In order to use the gyro and accelerometer measurements, it is 
necessary to have an accurate measurement of the orientation of the 
body frame with respect to the reference frame. The most common 
representation of this orientation is that of the direction cosine 
•matrix. This is a matrix whose elements are the cosines ‘of the angles 
between the axes of the body and reference frame. There are, however, 
other representations which can be used. Specifically, two of these 
are; (1) Euler parameters; and (2) Cayley-Klein parameters. The 
three techniques mentioned above are all governed by differential 
equations which are functions of vehicle rotations , and these 
equations can be solved by numerical integration on a computer using 
the gyro signals to indicate changes in vehicle orientation. The 
equations programmed in the computer for this purpose are called 
attitude algorithms . 

It will be the purpose of this thesis to evaluate, by use of 
comp'uter simulation, the relative merits of the three techniques 
mentioned above. The algorithms will be simulated as if they v;ere 
computed on a general purpose computer. Another technique employing 
Euler angles to represent the relative orientation of the coordinate 
frames was not included in this analysis due to the singularities 
inherent with three parameter schemes . These singularities are the 
equivalent of "gimbal lock" situations found in physical represen- 



The merits of each 


tations employing a three gimbal system- ’■ ■' 
algorithm can be determined by comparing the relative orientation 
computed by using each of the techniques (when specific vehicle maneuvers 
are prescribed) with the closed form solution of the direction cosine 
matrix. The accuracy of each of the a]gorithms will depend not only 
on vehicle maneuvers, but also on the integration schemes used to 
evaluate the differential equations and the rate extraction techniques 
used to determine the angular velocity of the vehicle. It will be 
assumed that integrating gyros which are sampled a number of times over 
each integration step will be utilized. These changes in accuracy by 
use of more sampling times or higher order numerical integration 
techniques will, of course, increase the computer time needed for each 
integration interval. The algorithms will be compared by: (1) deter- 

mining relative accuracy for each algorithm using different integration 
routines (Runge-Kutta first order, second order, and fourth order) , 
and specified vehicle motions; (2) determining relative accuracy for 
each algorithm when combined with an orthonormalization scheme; and (3) 
determining relative accuracy for each algorithm using different 
specified vehicle motions. 

The accuracy will be determined by putting the results of all 
the techniques into their equivalent direction cosine matrices 
(D.C.M.'s). Because of the errors incurred in numerical integration, 
these D.C.M.'s may not be orthonormal; and different orthonormalization 
techniques will be used to determine which yields the best results. 

After orthonormalization, the D.C.M.'s calculated by using the two 
different algorithms will be compared by determining hov; large a rota- 
tion must be made with each computed reference frame to align it v?ith 
the true reference frame. 

The vehicle motions which will be used for this analysis are 
those for which closed form solution can be evaluated. These motions 
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v^ill include: (1) single axis constant rotations; (2) three axis 

constant rotations; (3) single axis sinusoidal rotations; and (4) 
general coning motion. For each of these motions and different 
angular velocities, the accuracies of each algorithm v;ill be compared. 

The direction cosine algorithm computation using B = Bfl, (v;here 
a is the skew-symmetric form of the angular velocity of the body with 
respect to the reference frame, coordinatized in the body frame) seems 
to be the one most frequently used in strapdov;n systems . This thesis 
will try to either confirm the value of this algorithm or suggest the 
use of an alternative algorithm in order to yield the best results in 
a - strapdown sys tern . 
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CHAPTER 2 . 

Transformation Algorithms 

The purpose of a transformation matrix is to transform the 
coordinates of a vector in one frame to their associated values in a 
second frame. 

The transformation matrix accomplishes this transformation by a 
simple multiplication. If we let be the coordinates of a vector in 
the body frame, be the coordinates of the same vector in the 
inertial reference frame, and be the coordinate transformation 
from the body' frame to the reference frame, then; 

= ci 

— D “ 

where each element of the transformation matrix ci is the cosine of 

b 

the angle between an axis of the reference frame and an axis of the body 
frame. 

The purpose of a transfoiiiLatiou aigorithni is to uetenuint; the 
elements of the transformation matrix as a function of time, given 
only the value of the elements of the transformation matrix at the 
initial time”(t^) , and the angular motions of the body with respect to 
the reference frame coordinatized in the body frame. The latter can 
be obtained directly from the gyros mounted on the vehicle. 

If the angular rotation of the body is; 


u(t) = 


co^(t) 

(UyCt) 

o^(t) 
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where to , to , to are the angular rates about the x, y, z body axis 
respectively , then 


c^(t) = cj(t) n(t) (2.1) 

where is the skew symmetric form of the angular rate vector, i.e., 



Equation .2.1 actually represents nine first order coupled differ- 
ential equations. It is assumed that ^"0^ and w(t) are known. 
Therefore, (t) can be obtained for all t by numerically integrating 
these equations on a general purpose computer. This scheme will be 
identified as the "Direction Cosine Matrix Algorithm," and should not 
be confused v?ith the direction cosine matrix itself . 

The Euler parameter algorithm uses a four parameter technique . 

When Euler formulated this technique, he found that by using three 
parameters instead of two to represent the components of an axis about 
which one coordinate frame v;as rotating with respect to another, he 
could avoid the problem of singularities. A complete derivation of this 
technique can be found in Reference 2 . 

Instead of having nine differential equations to solve, the Euler 
parameter technique has only four. The Euler parameter technique can 
be summarized by the following equations: 
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Let e be the vector whose coitiponents are the Euler parameters 



This vector will then, satisfy the following differential equation 



and -che direction cosine matrix car. be obtarnecl -troir. purely algebraic 
manipulations as follows 


ej-tej-e^-e^ 2(6^62 +638,) 2(8^03-820^) 


c^= 2(0^02-930,) -e?+e^e5-0^ 2<e2V®i94J 


2(8183 +8264) 2(0203 -^164^ 


Using this technique, we have to integrate only four differential 
equations. However, these four values must be put into the D.C.M. 
form in order to use their information as a transformation from the 
body frame to the reference frame. The effort involved in accomplishing 
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this task is comprised solely o£ simple algebraic manipulations and 
is not an iterative procedure, as is the integration of the differential 
equations . 

Finally, the third, algorithm to be mentioned is that using Cayley- 
Klein parameters. It is also a four parameter technique satisfying 
the differential equation. ' 


d 

dt 



1 

2 





0 - 0 ) 

X 

u 0 

X 


-w 


y 


u 


z 


- 


- 

“W 



X 


1 

0) 


ct^ 

y 


2 

-CO 


a- 

z 


3 

0 


.“ 4 . 

— 



and the direction cosine matrix can be formed according to the 
following equation. 


^b = 




2 (-a^a 3 +a 2 « 4 ) 


2 (-a^a 2 +a 3 a^) 


2 2 ^ 2 2 
“l~“ 2 '^“ 3""4 


2 (ot^a^+a2“3^ 


2 (aj^H2+a2Ct4) 


2 (-«j^a^+a2a3) 


2 ^ 2 2 2 
“ l ’’’“2 “3 


If we let 
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we then have identical differential equations and identical forms for 
1 

the transformation matrix in both the Euler parameter and the Cayley- 
Klein parameter techniques. 

Because these two techniques are really the same, the computer 
analyses run on the IBM 360 were only programmed for Euler parameters. 
The results for the Cayley-Klein parameters would have been identical 
to these . There is no practical reason why one of the four parameter 
techniques should be favored over the other, with respect to accuracy, 
ease of programming, or computer time needed to process the algorithm. 
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CHAPTER 3. 

Rate Extraction, Integration Routines, 
and Orthonorraalization Routines 

A. Rate Extraction 

It was shown that the attitude algorithm requires continual 
knowledge of u{t) . However, most systems use pulse torqued integrating 
gyros v/hose output is not in the form of angular velocities, but rather 
in the incremental form A9 (t) , where A6(t) is an incremental change in 
angular orientation. It is necessary to be able to extract rate infor- 
mation m(t) from the series of pulse trains A9(t) for the Runge-Kutta 
integration routines which were used to integrate the algorithms ' 
differential equations. A first order approach to rate extraction 
would be to let 


_ <36 _ lim A6(t+At) -A0{t) 

^ ' dt At+0 At ^ 

Some integration routines, however, also need rate information at 
the midpoint of the integration interval, i.e., ci)(t+^). For this 
reason, a second order rate extraction scheme was used in the computer 
simulations which were implemented. 

This second order scheme samples ‘the gyro output at times t. 

At 

t + and t + At. Using these three points, and simulating the gyro 

output increments as 

6(t + ^) - 0{t) 

0 (t + At) - 0 (t + ~) , 

v?here 6 (t) is the actual simulated rotation, and fitting the points 
with a second order polynomial, yields after differentiation: 


AOj^Ct) = 
A02(t) = 
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<J(t) 


• 5A6j^(t) - A02(t) 


At 


• A0t ( t) + A9,(t) 

W(t + % = — i - 

^ At 


• -A0, (t) + 3A9_ (t) 

0)(t + At) = 

At 


Other techniques using higher sampling rates for the gyros can 
yield better approximations to w(t) than the method just described. 

The advantages and disadvantages of these techniques well be discussed 
later. 

B . Integration Schemes 

The rate information determined in the previous section can now 
be used as the input to the integration routines , which were used to 
get solutions from the algorithms' differential equations. In the 
computer simulatior‘=: pei"f ormed . it was arbitrarily decided to let the 
initial conditions be sot by the fact that at t = 0 the body frame of 
the Vehicle is exactly aligned with the reference frame. The magnitudes 
and general form of the -errors associated with the algorithms VJOuld not 
be changed by using other initial conditions, and exact initial align- 
ment was chosen merely for convenience. For the direction cosine matrix 
technique (t) = (t)fi(t), the initial conditions imply that 

(tfl) equals the identity matrix. For the Euler parameter technique, 
the initial conditions implying exact alignment are: 

0 ^ = 0.0 
02 = 0.0 
©3 = 0.0 
O4 = 1.0 
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For the purposes of this simulation, it vjas decided to test 
both algorithms using a first, second, and fourth order Runge~Kutta 
integration routine with fixed time steps of integration. Third order 
routines v;ere not employed, due to the fact that some of the cases 
simulated in this analysis have been previously analyzed using closed- 

form analytic techniques when first, second, and third order integration 

f 2] 

routines were employed. It was hoped that use of first, second and 

fourth order techniques in this analysis v7ould computationally confirm 
and extend the results of previous analytic comparisons. On an actual 
flight, it would be advantageous to be able to sample the gyros at a 
predetermined rate and also compute over predetermined intervals of 
integration, so that the amounts of computer time needed to implement 
a particular algorithm would be known exactly. The use of a variable 
step size integration routine would not permit the evaluation of the 
computation time needed because it would be impossible to determine 
beforehand what integration step size would be used. 

Runge-Kutta integration techniques were employed because they do 
not require past histories of the dependent variables, and they have 
an accuracy equivalent to the accuracy of a Taylor series solution of 
the same order. 

The equations governing these integration routines are shown 

below. 

Letting x(t) = f [x(t) , w(tl ] , the equations become: 

First-Order Runge-Kutta 

x(t + At) - x(t) + At If(x(t), u(t) ) 3 

Second-Order Runge-Kutta 

y = X (t) + Af [f (x(t) , w(t) ) 3 

x(t + At) = x(t) + ^ f (x(t) , u(t))] 
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Fourth-Order Runge-Kutta 

A = At f fx(t) , w(t))‘] 

B ~ At f[x(t) + u(t + ^) ] 

C « At f [x(fc) + |, u(t + ] 

D = At f[x(t) + C, u(t + Atl ] 

X(t + At) = x(t) + ^ ^ + 


D 

cTo 


C. Orthonormalization 

The elements of the direction cosine matrices computed by either 
the direction -cosine matrix technique or the Euler parameter technique 
will contain errors due to the fact that numerical integration routines 
are not exact. Because of these errors, the computed D.C.M.'s will not 
necessarily have the property of being orthonormal. 

Orthonormality is a requisite of a true D.C.M. and can be 

follows. If ; i = 1,2,3; are the three rows of the 

D. C.M. , normality requires that 


|C^1 =1 i = 1,2,3 
and orthogonality requires that 

it £2 " ^3 
£2 ^ £3 ~ £i 
£3 ^ £1 = ^2 


v;here "x" denotes the cross product operator. The algorithms used in 

I 

this analysis were also tested with the orthonormalisation routines 
included, to determine how much improvement could be obtained in the 
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For the Euler parameter technique, orthonormalization merely 
requires that: 

4 , 

I (0-)^ - 1 

i=l 

If this requirement is met, the rov;s and columns of the D.C.M. 
formed from the Euler parameters have magnitude equal to one, and they 
are mutually orthogonal. Therefore, the D.C.M. formed by these para- 
meters will be automatically orthonormal. Therefore, in the computed 
simulation, orthonormalisation was achieved by dividing each of the 
computed parameters by the square root of the sum of the squares of 
the computed parameters . 

Orthonormalization for the direction cosine matrix technique 
is not nearly as simple as for the four parameter algorithm. For 
this case, a technique was used which was not too complicated but 
does normalize the computed D.C.M. and orthogonalizes the computed 

^ ^ . . — ^ ^ .i.t. ^ ^ ^ 1 ^ ^ 

L.O J.liCt.1. 0.1. ^ a. J. ^ ^ 

Appendix 3) . 

The procedure used was to let i = 1,2,3; be the rov;s of the 

computed D.C.M. 

Then form: 


C. = B. X C, 
—1 -g -k 


where i=l, j=2, k=3; i 
k = 2. Next let 


-2, j=3, k=l; and i = 3 , j = 1 , 


B . = 
—1 


£j * £i 

l»i * 
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and the are nov; the rovjs of the orthonormalized D.C.I-i. 

The direction cosine matrices v.’ere orthonormalized each time 
the direction cosine matrix v?as formed, i.e. every ten seconds for 
the simulations performed. 
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CHAPTER 4. 


Error Quantities and Truth Models 
A. Error Quantities 

. Use of a computer" to integrate the differential equations of the 
attitude algorithms discussed in the previous chapter, yields a computed 
direction cosine matrix. The elements of the computed direction cosine 
matrix will differ from those of the true direction cosine matrix 
representing the relative orientation of the body frame and uhe refer- 
ence frame, due to errors in the numerical integration techniques, 
errors in the rate extraction schemes and numerical round off in the 
computer. In the present analysis, the errors due to numerical round 
off were reduced enough to become insignificant by using double preci- 
sion in all the programs run on the computer. Merely comparing the 
difference of the elements of these two matrices does not, however, 
yield a good physical representation of the errors in the computed 
D.C.M. 

TTv> ^ •»*■ ^ ^ Vi. 4- ^ -I jr* ,-iT V i-\ -y- V* ^ 4“ ^ ■+■ Vl £1 

errors associated with the computed direction cosine matrix, the fol- 

n 21 

lowing scheme can be used- ' 

Let C be the computed D.C.M. betv;een the body and reference frames, 

T 

C be the exact D.C.M. between the two frames, and C be the transpose 
of C. 

Then 


C = C + 6C 


or 

C = {I + E)C 
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where 


T 

E = 6CC 


T m 

and the fact that CC := I yields E = CC - I. The error matrix, E, 
will be of the form: 



The elements of the error matrix can be given a physical 
interpretation. The elements along the main diagonal indicate to 
first order the grovjth or decrement, of the column they are in, from 
unity. This error is called the "scale error" and is a dimensionless 

u uu.i« ^ ^ <-_2 • 

Letting be the element in the i^^column and i^^ row of 

the error matrix, "skew errors" can be defined as the average of the 
off diagonal elements of the error matrix, i.e. 

| k = l, -i=2, j = 3 
k=2, i=l, j=3 
k=3, i=l, j=2 

Physically the skew error is a measure of the perpendicularity 
of the i and j axes, as shown by the following figure: 
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j TRUE 


COMP 



The drift error is a measure of how much the computed frame must 
he rotated ahout the a»is to aiion it with the tr'xe fr^me- 

A geometric interpretation of the drift error appears below. 


j TRUE 
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The scale errors as defined are dimensionless quantities . The 
shew and drift errors are in radians, but can easily be converted to 
degrees, as was done for tliis simulation. 

Using these errors as a measure of accuracy for the different 
algorithms tested easily lends physical insight to the determination 
of the merxts of each algorithm. 

B. Truth Models and Input Rates 

In order to calculate the error matrix E, described in the pre- 
vious section, it is necessary to have knowledge of the true D.C.fi. 
Pour basic types of vehicle rotations were used in this computer ana- 
lysis, and a truth model which could compute the true D.C.M. in 
closed form was needed. 

.1. Single Axis, Constant Rate 

For the" single axis constant rate rotations, the D.C.M. is 
merely a function of the angle through which the body axis was 
rotated. Assuming an x, y, 2 coordinate sys em, the simulated 
rotations were input abour the y axis, i.e. 

! 0 \ 

k > k = cons.tant 

0 ) 

then ^ 0 

e^(t) = 0Q + kt e^(t) = 0 

0,(t) = 0 
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and 


cl (t) 

True 


f 


cos 0 (t) 

y 


-sin 0y(t) 


■0 

1 

0 


sin 0 

y 



0 


cos 0 (t), 

• y V 


2. Single Axis, Sinusoidal Rates 

The single axis, sinusoidal rates case also used the y 
body axis as the axis of .rotation. The form of the sinusoidal 
input used was: 


(o(t) = acos 0t 


As for the single axis case, the true D.C.M. is only a 
:ion of 0^(t) , \ 
following equation: 


function of 0^(t) , which was evaluated according to the 


0y(t) = 0Q(t) + J" acos 6t = 0 + ^ sin 0u 
^0 


Therefore 

0 (t) = ^ sin 0t 4.1 

Y p 

and the construction of the true D.C.M. is the same as for the 
single axis case with equation 3.1 being used to evaluate 

9y(t). 
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3 . Three Axes , Constant Rates 

For this case, a constant angular rate was input into all 
three axes. To evaluate the truth model for this case, we let 

C{t) = C(t)£J(t) 

true true 

which is the differential equation used in the direction cosine 
matrix technique . 

If we let $(t,t^) be the transition marrix for this equation, 
it can be shown (see Appendix A) that the_ true direction cosine 
matrix can be represented as : 


(t) = [I + ^ (1 - cos wt) - — sin wt]'*’ 

B. to 

true . 0 ) 

1 

where to = [u ^(t) + u ^ (t) t w ^ (t) ] ^ 

X y 

and fi(t) = skew symmetric form of m(t) . 

4 . Coning Motion Superimposed on Constant Pitch Rate 
The angular input for this case is: 


to(t) 


a sin Bt 
Y 

a cos gt 


For this case, it can be shown (see Appendix A) that the true 
D.C.M. as a function of time is as follows; 


(t) = [I + ^ (1 - cos (ot) + ^ sin cut] (t] 
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where 


u(t) 


0 

B + Y 
cx 


Id = -f- (P + Y)^)^ 


n(t) is the skevj symmetric form of <d(t) 


(, 


and Cg (t) 


cos 3t 


-sin gt 
0 


\ 


^in Bt 


0 


cos 



Besides evaluating truth models for the analysis, v^e must 
provide our computational algorithms with pulses i^(t) , in 
order to enable evaluation of the angular velocity, wCI;) , as it 
would be accomplished by measuring A£(t) from the gyro outputs. 
The A^(t) used for the analysis V7as not quantized, as would be 
the case with a physical system, • although evaluation of errors 
from this source might be desirable if the level of quantization 
were so large as to compete with the drift errors which come 
about from only approximate integration of the algorithm's 
differential equations . 


a. . For the single axis constant rate case 


u(t) 


0 

k 

0 


Ae^j^(t) = A 62 (t) 



0 

k At 

2 
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Thus kAt was the pulse input to the algorithm which then used 
the rate extraction routine described in Chapter 2 to derive rate 
information . 

b" For the three axes constant rate ca^e 


w(t) 



a 

e 

7 


and the pulses used by the algorithms were 


t+At 
2 


A0j^(t) = A02>{t) 


-I 


u)(t) = 


I ctAt 
2 

g At 

2 

YAt 
\ 2 


c. For the single axis sinusoidal case. 


to(t) = 


a cos St ' 
0 


and therefore 



0 



0 

AGj^Ct) “ 

t+At 

f ^ 

1 a cos gt dt 


a 

e 

At 

sin g<t + — 2 ") ” 


J 

t 

! 




0 



0 
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Ae^ct) 





t 4 

At 


/ 

a cos Bt dt 

= 

t 4 

At 



2 



0 



^[sin6(t + At)-sin$(t + J 


0 


<i. For the coning motion superimposed on a pitch rate, the 
angular rate is 


tfl(t) = 


a sin pt 


a cos 


and therefore 


A^^(t) = 


[cos e(t) - cos 3(t + ^) ] 


At 


^ [sin e(t + “) - sin ^t] 


I [cos 3(t + ^-) 


- cos p {t + At) ] 


A02(t) 


At 


^ (sin P(t + At) 


sin B (t + ] 
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It should be reemphasized that the angular rates could have been 
supplied to the algorithm directly? but doing uhis would have neglected 
the errors incurred by the algorithms due to the fact that they do 
not have exact means of rate extraction. 
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CHAPTER 5 


Results of the Computer Analysis 

In order to compare the accuracies of the previously discussed 
algorithms, four types of vehicle motions were simulated. For each 
simulated vehicle maneuver, both the direccion cosine matrir and Euler 
parameter techniques were employed using first, second and fourth 
order integration routines . The integration inuerval was kept at a 
constant value of 1/8 second, and all of the test runs were simulated 
for a period of three minutes . 

The drift, scale, and skew errors found in each of the computed 
D.C.M. 's v?ere then compared in an attempt to evaluate the relative 
merits of each of the algorithms tested. Although each type of vehicle 
maneuver was input separately, the generality of the programs allow 
any combination of vehicle maneuvers to be input, as long as it i.s 
still possible to formulate a closed form solution of the D.C.E>3. to use 
as a truth model. It was felt, however., that the maneuvers which 
were used for the present analysis are representative of common 
motions an actual vehicle would undergo during flight. 

A. Single Axis Constant Rate Case 

As previously discussed, a constant angular velocity of 10°/ 
second around only one body axis was the simulated vehicle maneuver 
for this test case. For this type of input, the rate extraction routine 
will yield perfect race information in. the absence of gyro pulse 
quantization. Therefore, the errors found in the computed direction 
cosine matrices will only depend upon the type of attitude algorithm 
employed, and upon the order of the integration technique used to 
integrate the differential equations of the algorithm. Errors which 
might have resulted due to the fact that digital computers only have 
finite word length, were made insignificant with the simulations 


33 



performed by using double precision word length with all of the computer 
programs . 

For this simulated vehicle maneuver, it was found that both the 
scale and drift errors grew linearly as a function of time, independent 
of the algorithm and integration routine employed to compute the D.C.M. 
Also, the skew errors were identically zero over the complete simulation 
period. 

These same simulations were also run with an orthonormalization 
routine included in the algorithm. The orthonormalization routines 
were used to orthonormalize the computed D.C.M. every five seconds 
during the three minute simulated maneuver time. It was found that 
the use .of an orthonormalization scheme decreased the scale errors by 
a factor on the order of 10~^ for those algorithms using a first or 
second order integration routine. The addition of an orthonorraalization 
scheme to the algorithms using a fourth order integration scheme did 
not yield a significant improvement in the scale errors. It is 
imporT:a!iu to note that the crthonormalization schemes did not improve 
the drift errors incurred by the algorithms , and in some cases they 
actually increased the drift errors (see Appendix B) . 

Orthonormali'zation-routines which were only employed at the end 
of the three minute simulation period were also analyzed. It V7as- found 
that orthonorraalization for those oases yielded much less significant 
improvement in scale errors than did orthonormalization routines which 
were used continuously throughout the simulations. 

If, however, it is required that the drift errors should be 
small, i.e., on the order of the size of the quantization level of a 
typical gyro pulse (approximately .007°), it- is found that the addition 
of an orthonormalization routine is of no advantage. The drift errors 
are not improved, and the scale errors are already in the 10 range. 

The single axis constant input test case showed the drift errors 
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incurred by the Euler parameter technique with any order integration 
routine tested v;ere always lov;er than the drift errors incurred by the 
direction cosine matrix technique .using the same order integration 
routine. Euler parameters yield 1/4 the drift errors that the direction 
cosine matrix scheme does for both the first and second order integra- 
tion techniques, and 1/16 the drift error for the fourth order 
integration techniques. 

Vfith orthonormalization every 5 seconds included, the drift 
errors at the end of three minures, normalized with respect to the 
drift error incurred by the direction cosine matrix technique using 
a first order integration routine, (0.285°), can be summarized 
as follows: 


Table 1. 


Drift Error 

1st Order 

2nd Order 

4 th 

Order 

-4 

B = 

1 

.5 

.12 

X 10 

E.P. 

.25 

■ .125 

.75 

X 10 ^ 


B = represents the direction cosine matrix technique. 

E.P. represents the Euler parameter technique. 

(0 = 10°/sec.; At = 1/8 sec. 

The elements of Table 1 were found to be constant for first and 
second order integration techniques, for varying integration interval, 
At, and for varying angular rate, to. However, the fourth order 
techniques were shown to improve relative to the first and second order 
techniques as the ratio w/At was decreased. The fourth order Euler 
parameters continued, however, 'to incur only 1/16 the drift error 
incurred by fourth order direction cosine matrix techniques as w/At 
was decreased. 
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B. Three Axis Constam: Sate Case 


In this test case, constant angular rates of 10°/second were 
input along the three orthogonal body axes. The resulting error 
analysis showed the scale, skew, and drift errors to be the same along 
all three axes. As in tho single axis case, it was found that requiring 
the drift errors to be small made the use of an orthonormaliaarion 
scheme to reduce scale and skew errors unnecessary. The drift for 
this case using D.C.M. technique with a first order integration scheme 
was found to be 2.391°. 

A summary of the normalized drift errors can be seen in Table 2. 


Drift Order 

E = BJ2 

E.P. 

Again it can be seen that for first and second order integration 
routines, the Euler parameter technique yields a drift error of 1/4 
the drift error incurred by the direction cosine matrix technique} and 
for fourth order integration routines, the drift error ratio is 1/16. 
It should also be noted that the fourth order integration techniques 
in this case do not have the same relative improvement over first and 
second order techniques as they did in the single axis constant rate 
constant rate test case. Their relative improvement was reduced by a 
factor of three from its value in the single axis case, although for 
the value of m/At used they still held a commanding advantage. 

If the results of these first two test cases are compared v;ith 
the analytical work of McKern, it can be seen that using a second 
order integration routine with either Euler parameters or direction 


Table 2. 


1st Order 


2nd Order 


4th Order 


.25 


.5 

.125 


.26 -X 10 


.225 X 10 


-5 
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cosine matrix schemes yields drift errors of half -those which would 
result from using a first order integration routine with the algorithms, 
independent of the value oj/At. Use of a fourth order routine, 
however, yields only 1/4 the drift error of a third order routine, also 
independent of xhe value u/At. 


C. single Axis Sinusoidal Rate Case 


For this test case, the simulated vehicle angular rate was of the 
form (0 = S ccsgt. For computer runs made, using a simulated vehicle 
rate of this form meant that by varying B, the vehicle would perform 
constant amplitude but varying frequency oscillations . 

The results of the error analysis showed that there was no skevr 
error incurred, and that the scale error did not need improvement by 
an orthonormalization routine when the drift errors were kept small . 
Most important v?as the result that the_ drift errors for this case did 
not grow linearly with time as in the constant rate test cases, but 
instead were bounded sinusoids having the same period as did sinBt. 


The drifu errors lor this case had another interesting dependence 
upon j5. For the first order integration schemes, using either Euler 
parameters or the direction cosine matrix technique algorithms, drift 
errors were directly proportional to' 3. With second order integration 
techniques, the drift- errors for both algorithms were proportional to 
3^, and for fourth order integration techniques the drift errors were 
proportional to 3^. No computer runs were made with a third order 
integration routine, but it seems reasonable to speculate that the 

3 

drift errors for a third order routine would be proportional to 3 • 

The ratios of Euler parameter drift errors vs. direction cosine 
matrix drift errors for this case is shown in Table 3. 
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Table 3 . 


Drift Errors 1st Order 2nd Order 4th Order 

E.P./B = Bf2 1 1/4 1/16 

These ratios held constant as the value of 3 was varied. These 
ratios are the same as they are for both the single and three axis 
constant rate cases when second or fourth order integration routines 
are used. However, the advantage held by the Euler parameters decreased 
sharply for the first order integration routines . 

' D- Coning Motion Superimposed on a Constant Pitch Rate 

For this simulation, the simulated angular rate of the vehicle 

was; 


a sin 3t 


u(t) = 


a cos 6t 


As in the sinusoidal rate_case, the magniutdes of a and 3 were 
kept the same. 

The results of the computer analysis showed the drift errors 
incurred along all three axes to be irregular functions of time, i.e., 
they vjere not linear and they did not have any simple sinusoidal form. 
In order to compare the accuracies of the algorithms tested, a mean 
drift was defined as follovjs: 

1 

^5ean Drift = (D^ + D^ + 
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where D^, D^, was the maximuni drift achieved during the three 
minute simulations about the y, and z axes respectively. Two 
basic test inputs were simulated for this case. The first used 


ot = Y = l°/seo. e =.l 

and the second used 


a ~ Y = 10° /sec. 3 = 10.0 

The normalized mean drift errors is shown in Tables 4 and 5. The 

mean drifts for these cases v;hen the D.C.M. technique with first order 

. . - -2 ~1 • 
integration was employed were 6.25 x 10 and 10.2 x 10 degrees 

respectively. 

Table 4 . 

Normalized Mean Drift Errors 

1st Order 2nd Order 4th Order 


B = 

1 

.71 X 

io“2 

.30 X 

io“8 

E.P. 

■ 1 

* .27 X 

io“^ 

.16 X 

lo"^ 


a = 3 = Y = 1 


Table 5. 

Normalized Mean Drift Errors 



1st Order 

B = Bfl 

1 

E.P. 

.66 


P 

II 

105 

1) 


2nd Order 

4 th 

Order 


.19 

-4 

.44 

X 10 

.16 

.23 

X 10“° 


Y = 10 
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The most significant result of the analysis is that again Euler 
parameters had mean drift errors less than or equal to those incurred 
by the direction cosine matrix technique, when the algorithms were 
tested with the same simulated input and evaluated with the same order 
integration technique. 

The coupling of the algorithms' differential equations, and 
dissimilarity of the angular rates imposed upon each axis for this test 
case, made it difficult to derive a concise correlation between the 
magnitudes of the inputs and the resulting mean drift errors incurred 
by the algorithms . 
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CHAPTER 6 


Conclusions 

For every case tested f the Euler parameter algorithm proved to 
be at least as good as, and in most cases superior to the direction 
cosine matrix algorithm, when both used the same order integration 
techniques. It would seem wiser, therefore, to choose a four parameter 
technique in favor of the direction cosine matrix scheme which employs 
nine differential equations. 

Although the orthonormalization algorithm used with the direction 
cosine matrix algorithm was only an approximate scheme, the method 
employed with the Euler parameter algorithm was exact, and showed 
that orthonormalization was useful only for reducing scale and skew 
errors . It further proved orthonormalization to be completely unneces- 
sary when the algorithm employed uses An integration interval small 
enough to maintain the drift errors at a magnitude on the order of the 
quantization level of a typical gyro pulse. For those reasons, it 
would seem appropriate not to waste computer time by employing an ortho-- 
normalization routine when the above conditions are met. 

It was mentioned in a previous chapter that the rate extraction 
routine used for these simulations was not the only choice available. 

One could in fact sample the gyro outputs more often over the integra- 
tion interval in order to derive more accurate rata information. For 
example, the gyros' outputs could be sampled more frequently and be 
fitted with a third rather than a second order polynomial. Hov/ever, 
choice of the optimum rate extraction scheme to be used vjould depend 
upon a priori knowledge of the angular rates to be undergone by the 
vehicle. The simulations run for tjiis paper v;ere not employed to 
explore the advantages of more accurate rate extraction routines . 
However, the generality of the computer programs written for this 
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analysis could easily handle changes such as this . It might be 
useful, therefore, in future work to explore the effect of rate 
extraction upon the accuracies of the attitude algorithms. 

It must .-also be reemphasized that the simulation runs employed 
double precision throughout. This yields an accuracy of 15 decimal 
digits for each number stored in the computer. When some of the simu- 
lations were carried out in single precision (accuracy 7 decimal 
digits) , it V7as found that computer round off became one of the most 
prominent sources of inaccuracy in the algorithms. Typical on-board 
computers. do not have as long a word length as does the l.B.M. 360 
with double precision. The accuracy of the on-board computer will, 
therefore, be a great source of concern when attempting to implement 
an attitude algorithm with a strapdovm inertial navigation system. 

Another important aspect concerning the accuracy of an attitude 
algorithm is the fact that each pulse from an integrating gyro is 
obtained only after the vehicle has undergone a certain minimum 
change in angular orienuauion . This minimum change to produce a 
pulse can be identified as the pulse quantization level of the gyro 
being considered. This simulation did not take this quantization 
into account, but again the programs could easily be modified to do 
so. The pulse quantization level vjould set a minimum magnitude on 
the drift accuracy of the algorithms due to the fact that the algorithms 
cannot be expected to have accuracies greater than the information 
supplied to them. The build-up of errors due to gyro quantization could 
be a study by itself , but for the purpose of these simulations it v;as 
ignored. It would, however, be reasonable to assume that gyro quantiza- 
tion errors would only 'be important when algorithm drift errors are 
about the same order of magnitude as the quantization levels . For 
much larger drift errors, the errors caused by quantization vjould in 
all likelihood be overshadov.'ed by the errors incurred due to only 
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finite order numerical integration and errors in the rate extraction , 
schemes . It might be. possible to analyze effects of gyro quantization 
by merely looking at the effect they would have on rate extraction 
information. 

Finally, it must be noted that although higher order integration 
routines yield better accuracies, they also take more rime for 
computation-. In some applications it might be more commendable to use 
a low order integrarion routine and small integration interval rather 
than a high order integration routine and large integration interval. 
To make a good choice would depend upon knowledge of typical vehicle 
maneuvers for the application of the strapdown system computation 
times required by the general purpose computer being used, and the 
.accuracies required of the algorithm. . . 

Although this study does not indicate what is the optimum rate 
extraction or optimum order integration routine to employ, it does 
strongly indicate that the four parameter ‘techniques have a distinct 
advantage over the direction cosine matrix method, both of which yield 
attitude algorithms with no singularities . 
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APPEMDIX A. 

Closed Form Solutions for the Direction Cosine Matrix 
The differential equation for the D.C.M. is 

C(t) = C{t)JJ(t) , A.l 

This matrix equation actually represents nine first order differential 
equations and can be rewritten as 

C^(t) = n^(t)C^(t). A. 2 

The solution to Equation A.l -can be expressed using state space tech-’ 

• ■ [ 6 ) 

niques as 

C^(t) = ^>(t,t^)c'^(tQ). A. 3 

Throughout the remainder of this appendix the initial values will 
be set as follows 



C(tjj) = C(0) = Cp. 

The differential equation for the state -transition matrix in Equation 
A. 3 can be expressed as 

l(t,0) = -n(t) ^>(t,0) , A. 4 

because fJ(t) is skew symmetric, i.e. 

n'’^(t) = -n(t); A. 5 
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and the boundary condition for the' differential equation is 


.(0,0) = I 


A. 6 


The solution for the state transition matrix, when fJ{t) is,, 
constant, is 




i5>(t, 0) = e = e 


-ii(wt) 


" ,n, 2 ,wt, 2 ^ a 3,wt.3,^ 

= I + -(mt) + (-) (^) + (-) ijr) 


2 2 2 2 

where w = (u^ + + (o^) . 


A. 7 
A. 8 


Noting that 


^-- 2 ^ ,=tc 
S' — - -2' 
u m 


A. 9 


and substituting A. 9 into A. 7 yields 


$(t,0) = I -i^) [(mt) - 


, (mt)^ (wt) ^ , 

+ l-jj Ti + fi 1 + • : ■ J ' 


4! 


6! 


A. 10 


Now 




(mt)^ (mb) 


0(t,0) = I - ~[sin mt] - ^ [1 - ^2^ + 


4! 


..1 

w 

A. 11 


which results in 


4.(t,0) = I - 


-[1 r cos tot] 


A. 12 
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From Equation 7^,3, the D.C.M. is shown to be 


C(t) = Cp<i>-"(t,0) 


A, 13 


so that 


C(t) = C-, [I ~ —sin ut + — irCl - cos wt) ] 

u w Z * 

(Ji 


A. 14 


For the case where = I 


C(t) = [I + —sin tot + ^ (1 - cos wt) ] 

CO ^ 

03 


A. 15 


’ For the case of coning motion superimposed upon a constant pitch 
rate, the body angular velocity is given by 


r a sin pt 1 


b 

(0 - = 
“ib 


A, 16 


which can be expressed as 


-lb a -ib 


A. 17 


where 


C^(t) = 

Cl 


cos Bt 
0 

-sin Bt 


sin Bt 


cos Bt 


A. 18 
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and 



Nov; 


C^(t) = C^(t)C^(t) 


A. 19 


A. 20 


which is the matrix of interest for this case . 

It is nov; necessary to determine the solution for (t) . 

3 . 

From Equation , the value for can be determined as 


=S ss 

“3b — ab 


f 0 1 

-& 

0 


A, 21 



• 


( ° ^ 




b b 


B + Y 


1 

-6 


to • 
—lb a 


+ 




V- y 


y 0 / 




• ' 


and because 


a a 
to., = ( 0 . 
—lb — la 


-a.h 


it must follov; that 


A. 23 
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0 


— la 


e + Y 


CO. is of constant value and 
— la 


C^(t) = C^(t)n 


where 


0 = 


“(e + y) 


-ct 


(g + Y) 
0 


The solution to A. 25 is then given as 


C^(t) = [I + ^ (1 - cos wt) + ^ sin wt] 
a ^ CO 

CO 


where 


1 

u) = (a^ + (g + Y)^)^' 

The value for (t) is now given as 
2 

cf (t) = [I -h ^(1 - COS cot) + ^in cot] C?(t) . 

D ^ to O 

CO 


A. 24 


A. 25 


A. 26 


A. 27 


A. 28 
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APPENDIX B. 

Orthonormalization 

One technique that has been suggested for orthonormalizing. a 
computed direction cosine matrix is the following. 

Letting C be the computed D.C.K. and 
_1 

C* = C (C C) B.l 

then C* will be the optimal orthogonal approximation which minimizes 
Trace [(C* - C) (C* - C)^V ' E.? 

« 

Letting 

lA. 

C = C + 6C =(I + 6CC’^)C _ B.3 

where C is the true D.C.M., and expanding Equation B.l to first order 
according to the binomial series yields the result 

C* = ^ <5CC^ - I C6C‘]C. B.4 

Letting P = 6CC^ and substituting into B.4 gives 

C* = [3 + ip - B.5 

When this first order approxim.acion is substituted into the error 
equation of Chapter 4 

^ rn 

E = C C - I B.6 
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B.7 



Note that the right hand side of equation B.7 is in skew symmetric 
form. This means than the error matrix would be of rhe following form. 



This error matrix would yield zero scale and skew errors inde- 
pendent of the magnitude of its elements . 

Letting the difference matrix SC and the true D.C.M. be divided 
into rov/ vectors 
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■ and the three drift errors as defined in Chapter III are 


Drifr^ = 


— -T — -T 
^ 2*'=3 - ^ 3-^=2 


— -T - -T 


Drii 


B.ll 


Drift^ = 


- — T - — T 

a, • c„ — a„ • c, 
1 2 2 1 


If the computed D.C.M. is not orthonormalized, the error matrix of 
Equation B.2 becomes 


E = 


— -T 


- -T 
^2*^1 


- -T 


— -T 

^1*^2 


— -T 


- -T 
a ^ • c* 


— -T 
^1*°3 


_ -T 
®2*'^3 


- -<C 

a, • c. 

j j 


B,12 


and the other errors as defined in Chapter III are as follows 


ScalCj^ = 

-T. 2 

(ai-ci) 


/- -Tv 2 

(^ 2 '^i 5 

+ 

{a 3 *c^)^ 

Scale^ = 

m o 

(ai*c^2> 


-T. 2 
(a2-C2) 

+ 

(a3*E^)2 

Scale^ = 

t- 2 

(ai*C 3 ) 

+ 

-<P. 2 

(32*03) 

+ 



Skev?, 


- _T, - -T 

^ 2 '^3 ^3 •'=2 


B.13 


Skew, 


- -T, - -T 


Skew. 


- -T , — -T 
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- -T - -T 
^ 2 '= 3 - ® 3'=2 


Driftj^ = 


Drift, 


— — T - — n* 


Drift, 


rr it's: - -’S 

^12 ° 2'^1 


The drift errors of Equation B.13 are seen to be identical to 
those Of Equation B.IO, Therefore, an exact first order orthonorma- 
lization routine does not yield any benefit in reduction of drift 
errors. This is due ro the fact that the error matrix associated 
with the orthonorraalized D.C.M. is the skew symmetric form of the 
error matrix associated with the non-orthonormalized D.C.M., and 
drift errors are defined "as the skew symmetric portions of the error 
matrix. Obviously, skew symmetritizing an already skew symmetric 
matrix uotsS not change it. 

Therefore, first order orthonormalization routines will effec- 
tively null scale and skew errors. However, they do not minimize B.2 
as much as would a higher order expansion. 

A second order expansion of Equation B.l results in a C* of the 

form 

C* = [I - •* ^ 

T 

Where again P ~ 6CC . 

The error matrix for this second order approximation is then 


E 





1 T 


1 T 
— PP 
8' 


3 T T 
— P P 
8 ^ 



B*15 


56 



This error matrix is not of skew symmetric form; and thoraCore, 
the scale and skew errors will not be zero. However, if the higher 
order terms of the binomial expansion are included. Equation B.2 
should be more effectively minimized. This v?ould imply that a lesser 
amount of change v?as made upon the computed direction cosine matrix 
to orthonormalize it. 

The results of rhe proceeding analysis show scale and skew errors 
to bs second order, while drift errors are first order with respect 
to the errors, found in the computed direction cosine matrix. 
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C MAIN PROGRAM 

IMPLICIT REAL*B( A-G,0-Z) 

COMMON TINT.NCNT 
COMMON /ABC/ PLOT,PPRINT 
COMMON /CONG/ ALPHAtBETA, GAMMA 

DIMENSION BC(3,3J ,BT( 3t3) fOB(3,3J ,Oei3t3 ) .0THETAI3) , E(9) 
DIMENSION NTIPODO) ,E1(2000»,E2<2000),E3I2000) . 

DIMENSION E4 (2000 ) fESI 2000), E6( 2000 I ,E T( 2000 ) ,E8 ( 2000 ), £9( 2000) 
DIMENSION HEAD(IOO) 

DATA MARK/ IH* / 

IREAO = 5 
IHRITE = 6 

C INITIALIZE PROGRAM PARAMETERS 

PI=3, 141592653589732 
RDTODG = 10O»O/PI 
ALPHA = 1»0 ’ 

BETA=laO 
GAMMA = loO 
OGTORD = PI/180. 

ALPHA=ALPHA1'DGT0RD 

BETA=BETA>‘.«DGTORD 

GAMMA=GAMMA=i'DGTORO 

C READ SIX JOB DESCRIPTION CARDS 

READ(5,3) (HEADU ),I = 1,108) ' 

3 F0RMAT(18A4/1BA4/18A4/18A4/18A4/18A4) 

WRITE (IHRITE, 2) 

2 FORMAT (1H1,40X,35HGSSA2 - ATTITUDE SIMULATION PROGRAM// 

1 1K,15HJ0B DESCRIPTION//) 

WRITEI6.4) <HEAD( n, 1 = 1,108) 

4 F0RMAT(6(/2X,18A4) ) 
on 10 1=1,3 

bn 10 j=i,3 
BT(I,J) =OoO 
10 BC( I,J) =6.0 
BT(l.l) = 1.0 
• BC(l.l) = 1.0 


MAINOOOl 
MAIN0002 
MAIN0003 
MA-IN0004 
MAIN0005 
MAIN0006 
MAIN0007 
MAIN0008 
MAIN0009 
MAINOOlO 
MAINOOll 
MAIN 10 12 
MAINC013 
MAIN0014 
MAIN0015 
MAIN0016 
MAIN0017 
HAIN0018 
MAIN0019 
MAIN0020 
MAIN002i 
MAIN0022 
MAIN0023 
MAIN0024 
MAIN0C25 
MAI NO 02 6 
MAIN0027 
MAIN0028 
HAIN0029 
MAINOC’30 
HAIN0031 
MAIN0032 
HAIN0033 
MAIND034 
MAI NO 03 5 
MAIN0C36 


O 

I 

rt 

(D 

CO 


3 

C 

ft 

H- 

O 


o 



VI 




BT(2»2) = 1.0 

MAINOPai 


80(2.2) = 1.0 

MAIN0L3C 


8T(3.3) = 1.0 

MAIN003' 


BC(3,3) = 1.0 

MAIN004C 

; 

-READ INPUTS 

MAIN0041 


READ UREAD.402) F RECJ . TPLOT . TPR I NT ,T F ‘ 

MA1N0C4I 

40? 

FORMAT(4F10.01 

MAINOOA; 


WRITE UWRITE.403) 

MAIN004^ 

403 

FORMAT ( //'//iX, lOHINPUT DATA) 

MAIN0045 


WRITE n WRITE, 40<.) FREO.TPLOT, TPR INT »TF 

MAIN004f 

404 

FORMAT (/■/1X.6HFRE0 | = D15.8,4Xr7HTPL0T =D15.8,4X,8HTPRINT = 

MAI NO 041 


1 Dl5,a,4X.4HTF =1315.3//) 

MAIN004I: 


TINT = l.O/FREO 1 

MA IN004S 


C 

e 

O 

II 

1- 

HAIN005( 


NTd) = T 

HAINU051 


NPLT = 1 

MAINU05I 


PLOT = TPLOT 

MAIN005: 


PPRINT = TPRINT 

MAINOOS^ 


CALL ERROR ( BC, BT . D3 , OE , E , 1 WR IV E ) 

MAIN005I 


DO 20 K=4,9 

MAIN005I 


FIX) = EIKl'XROTnOG 

MAINOOSl 

20 

CONTINUE 

MAIN005I 


EltNPLTl = F(1J 

MAIN005' 


E2(NPLT) = E(2) 

MAINCOOf 


E3(NPLT) = E13) 

MAIN0061 


E4INPLT) = E(4» 

MAIN0061 


E5(NPLT) = E(5) • 

MAIN006' 


F6(NPLT) = E(6l 

HAINOOO-: 


E7(NPLT) = Et71 

MAIN006; 


EBINPLTJ = FI 81 

MAIN006I 


F9INPLT) ^ FI9! 

MAIN0C61 


WRITE (IWRITE.405) 

MAIN006! 

405 

FORMAT (////IX.llHOUTPUT DATA) 

MAIN006' 


WRITE (IWR!TE.406) T 

HAIN007( 

406 

FORMAT (///1X.6HTIME =D15.8) 

MA IN0O7] 


WRITE tIWRITE.407) 

MAIN007I 



407 FORMAT (//1X.37HBC - COMPUTED i)IPECTIGN COSINE MATRIX, 22X, 

I 33HRT “ TRUE DIRECTION COSINE MATRIX/) 

DO 21 1=1,3 

WRITE <IWRITS,40a) ( BC ( 1 , J I , J-1 , 3 I , { BT { I , J ) , J = 1 , 3 ) 

?1 CONTINUE 

40B FORMAT ( 3C1 6, 8 , 1 1 X . 30 16 . 8 ) 

WBITF (IWRITE.AOO) 

409 FORMAT (//1X,2?HDB - DIFFERENCE M AT K I X , 37X , 1 7U0 E - ERROR MATRIX/) 
on ?? 1=1,3 

WRITE ( IWRITF, 408) ( DB ( I , J = 1 , 3 ) , ( l)E <1 , J ) , J= 1 , 3) 

22 CONTINUE 

WPITF iIWPTTE,4n) 

410 FORMAT (//1X.RH6 VECT0R//6X . 21HSCALE ( D I MENS LONLE SS ) , 29X, 
llOHSKEW ! DFG) ,42X,11H0RI FT (DEG5/) 

DO 23 1=1,3 

WRITF UWRITS,4111 E ( I ) , E ( 1 + 3 ) , £ ( I +6 ) 

73 CONTINUE 

411 FORMAT (7X,D16.8,?9X,016i8,36X.,016.8) 

c SFT UP no LOOP LIMIT FOR MAIN LOOP 

XINT = TF/TINT 
INT = XINT 
I? = n 
NCMT = 1 

C MAIN LOOP 

100 I? = I? + 1 

CALL PULSE tT.TINT,DTHETAI 
T = T+TINT 

CALL ALGOR (DTHETA, 8C , IWR ITE ) 

IF {T .LT. PLOT) GO TO 31 
NPt T =.NPLT+1 
NT (NPLT) = PLCT 
PLOT = PLQT+TPIOT 
CALL truth (T,BT) 

CALL ERROR ( BC, BT , 00 , DE , E , I UR I YE ) 

DO 3? K=4,9 

eiK) = FUI’^RDTODG 


HAINU07: 
MAIN00 7- 
MAIN0071 
MAIN007< 
MA INU07' 
MA IN007) 
MAIN')07- 
MAIN008( 
M'AiNOoa: 
maiN'K'b; 
mainolb: 

MAIN008' 
MAIN003! 
MAIN0u8< 
MAINOOSl 
MAIN008I 
MAIN008' 
MAIN009( 
MA7N009) 
MAiNur>9; 
MAINOOP; 
MAIN009^ 
MAIN009! 
MA IN009« 
MAINOOgi 
MA1N009I 
MAIN009' 
MAINOlOf 
HAIN0101 
MAIN0102 
MAINOID; 
MAIN01U4 
MAINOIO' 
MAIN010« 
HAIN0107 
MAINOlOe 


o 



CT! 


^2 CONTINUE 

EKNPLn = E(1 > 

E2(NPLTI = e<2J 
E3(NPLT) = Ef3) 
e4(NPLT) = Et4l 
F.StNPLT) =■ F(5( 

E6(NPLT) = E(65 
E7{NPm = £(7) 

EP(NPLT) = E(8) 

E9(NPLT) = E(9» 

3i IF (T.LT,PPRINT) GO TO 131 
APt.T=Pl.OT-TPLOT 
IF(ToGE,APLT) GO TO 132 
CALL TROTH (T,BT> 

call error <BC. RT,DB»DE tE»IHRIT£) 

132 CONTINUF 

• PPRINT = PPRINT+TPRINT 

C WRITE OUTPUT 

WRITE (IKRITEf406) T 
WRITE nwRITE,407> 
on 24 1=1,3 

WRITE I IWRITE-.408) ( BC ( I , J ) , J=1 , 3 ) , ( RT ( I , J ) , J = 1 , 3 ) 
2T, CONTINUE 

WRITE (IHRITE,40gi 
00 25 1=1,3 

WRITE nWRITE,4Q8) I OB I I , J ) , J=1 , 3 } , ( l)E ( I , J 1 , J= 1 , 3 ) 

25 CONTINUE 

WRITE (IWRITE,410) 

00 26 1=1,3 

WRITE (IHRITG.411> E (I 1 , E < H-3 > , E < 1 + 6 ) 

?6 CONTINUE 
131 CONTINUE 

IF I 12 »LT» INTI GO TO 100 

C REAR SIX PLOT DESCRIPTION CARDS 

READ nREAD.3) I HE AO (1 ) , 1= 1 , 72 ) 

C PRINT PLOT DESCRIPTION HEADING 


MAIN0109 

MAINOllD 

MAINHll 

KAIN0112 

MAIN0113 

MAIN0114 

MAIN0115 

MAIN0116 

MAINOUT 

MAIN0U8 

KAIN0I19 

MAIN0120 

MAIN0121 

MAIN0I22 

MAIN0123 

MAI NO 124 

MAIN0125 

MAIN0126 

MA IN0127 

MAI NO 128 

MA1N0129 

HAINn30 

MAIN0131 

MAIN0132 

MAIN0133 

MAIN0134 

MAIN0135 

MAIN0136 

HA I NO 137 

MAIN0138 

MAIN0139 

MAIN3140 

HAINU141 

MA I NO 142 

MAI NO 143 

HAIN0144 



WRITI; nWRITEtS) 

5 FORMAT (IHl, 1X,16HPL0T DE SCR T PT I JNX / I 

WRITFUWR ITE,4C'<'f ) FP EO, TPLOT , TPk INT , TF 
WRITE (IWRITE,4) I HE AD ! I) , 1 = 1 , 7 ? ) 

WRITF (IHRITEt420l 

CALL PLCTER ( El , NT , NPl T , M ARK , I WR IV E ) 
WRITF (I WRITE. 421) 

CALL PLOTER ( F2 , NT , MPLT i MARK . IWR IT E ) 
WRITF ( IWRITE.422 ) | 

CALL PLOTER ( E 3 . N T , NPL T , MAR K , I WR I IF ) 
•WRITE nWRITE,423) 

CALL PLOTFR ( E4 , NT , NPLT , ^SAP K , IW R I T F ) 
WRITF { IWRITF.424 I 

CALL PLOTER ( E5 , NT , NP LT, MA^K . I WR IT E ) 
WRITE {IWPITF.425) 

CALL PIOTER ( EA.NT.NPLT.MARK, IWRIVE) 
WRITE (IWPITE.426) 

CALL PLOTFR I E7 . NT , NPLT , M ARK , I WR I T E » 
WRITE (IHRITF.427) 

CALL PIOTFR ( E8, NT. NPLT, MARK, IWRIfF ) 


MAIND145 
MAIN0146 
KAIN0147 
MAINC148 
MAIN0149 
MAIN.'ilSO 
MAIN0151 
MAIN0152 
MAINL'153 
HAINt'154 
MAINl'155 
MAIM0156 
MAIN0157 
MAINOISS 
MAIN0159 
MAINvl60 
MAIN0161 
MAIN:>i62 
MAI NO 163 
MAIN0164 


4?6 
42 7 
428 


WRITF ( IWRrTF.4?8 ) 

CALL PLOTER ( E9 , NT , NPLT .MAP K, I WR IV E ) 

IlHl .45X,50HX-A;<IS SCALE ERROR ( 0 I MENS lONLE SS) ■ VS. TIME IS 

(IHl .4SX,5THY-AXIS SCALE ERROR (.u I M ENS IQNL E SS ) VS. TIME (S 

nHl,45X,50H7-AXlS SCALE EliROk (DIMENSIONLESS) VS. TIME (S 

( IHl, 45X,40HX-Y SKEW FRROR (DEGREES) VS. TIME (SEC.)) 

( IHl .45X.40HY-Z SKEW ERRDR (DEGREES) VS. TIME (SEC.)) 

( IHl, 45X.40HZ-X SKEW FRROR (DEGREES) VS. TIME (SEC.)) 

FORMAT! IHl ,45X .45HDRIFT ERROR ABOUT X (DEGREES) VS. TIME (SEC.)) 
FORMAT! IHl, 49X .45HDRIFT ERROR AROUI Y (DEGREES) VS. TIME (SEC.)) 


420 

FORMAT 


IFC. ) ) 

421 

FORMAT 


1 FC. ) ) 

422 

FORMAT 


•lEC. ) ) 

42 3 

FORMAT 

424 

FORMAT 

425 

FORMAT 


FORMATdHl ,45X,45HDRIFT ERR13R ABOUT Z 

STOP 

END 


(DEGREES) VS. TIME (SEC,)) 


MA INOl 65 
MAIN0166 
MAIN0167 
MAIN0168 
main;; 169 
MA INOl 70 
MA INOl 71 
MAIN9172 
fMAINfa73 
MAIN'^174 
MAIN0175 
MAIN0176 
MA3NC177 
MAIN0178 
MAIN0179 
MAIN'S IQ -3 


o> 
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SUFSROUTINE FRROR ( BC, 8T, DR . DE ,E . 1 WRIT E) 

ERROOOOl 

IMPLICIT REAL"B( A-GtO-Z 1 

EKROOf'02 

DIMENSION BC(3,3),RT(3 

,3),F(9),0E(3,3) ,BTT(3.3) 

ERROOU03 

DIMENSION D8(3,3) 


ERROOOOA 

■COMPUTE 

ERROR MATRIX 


ERR00005 

CALL MATXT (BT,8TT) 


6RR00006 

CALL MATMLT' ( BC , 3 , 3. BTT , 3 1 3 t DE t I UR I TE ) 

ERR00007 

on 10 I 

=1.3 


ERROOOC8 

OEU. n 

= DEI I . I 1 - l«n 


ERROOf',19 

on 20 

1 = 1,3 


FRRO-'»on 

on 20 

J=1 .3 


ERROUOll 

0B( I . J) 

= fiCt I, J ! - 3T( I, J) 

ERRQ0012 

CONTINUE 


EBR00013 

Em = 

DEd.l) 


ERROOOIA 

F<2) = 

OE! 2, ?) 


ERR00015 

F13) = 

06(3,3 ) 


ERR00016 

6(A) = 

( DEI 2,1 )+DE( 1,2) 

)/2o0 

ERR00017 

E(5) = 

( 0£(2,3)+DE( 3, 2) 

1/2.0 

ERR00018 

616) = 

(D6(3.1)+DE(1 

,3) )/2,G 

ERR00019 

E ( 7 ) = 

{ DF( 2,3 )-06( 3, 2) )/2,0 

6RR00020 

E(8) = 

(DE(3,i )-DE( 1, 3) )/2.0 

ERR00021 

619) = 

(DE(1.2)-DE(2,1) )/2,0 

ERR00022 

RETURN 



ERR00023 

END 



ERR0002A 



SURPOUT INE 
tMPLir.IT PCAL'=a( A-G,0-Z) 

INTEGER BLANK, mr, PLUS 
DATA BLANK / IP / 

DATA DOT / IH. / 

DATA PLUS f 1H+ / 

DIMENSION PLOTINUMBPl ,NT(NUM8R) ,LIN£<1C.3 ) 
A8S( X)=OABS( X I 
DMAX=ABS:PLnTm ) 

DO 5 l=l,NUMHR 
XDATA = ABS( PLOT {1)1 
IF( DMAX-XDATAl-it.S.S 

4 DHAX=XDATA 

5 CONTINUE 
FACTK = OMAX 

IF( FACT P . FO. T.O) GO TO 13 . 

SPACE = Sn.O / FACTR 
HOG = FACTR 

HKITE ( IWRI TE, lOP ) HUG 
102 FORMAT ( /50 X , 17 HSC AL E (C TO K' ) = 

1 B6X,5HMINIJS,55X,4HPLUS/) 


DO I 1=1,103 
1 INF(I i=onT 

LINF 

(1 I 


1 

LINE 

(2) 


0 

LINE 

(7) 

= 

9 

1 INF 

1 12 ) 

:= 

8 

LINE 

(17) 

= 

7 

LINE 

(27) 


6 

LINE 

(27) 

= 

5 

LINE 

(32) 

=: 

4 

LINF 

(37) 

s: 

3 

LINE 

(4?) 

= 

2 

L I NE 

(47) 

= 

1 

LINE 

(52) 

- 

0 

LINF 

< 57) 

= 

1 


PLCTO)..01 
PL0T0002 
PLCTrif'03 
PL0TC'(>?4 
PL0TO005 
PLOTOOU6 
PLOT0007 
PLCT00D3 
PLOTOGOS 
PLQTOulO 
PLOTO'Ul 
PL0T0O12 
PL0T0D13 
PLUT0014 
PLOTOr 15 
PL0T0016 
PL0T0017 
PLOTOOIB 
PLOTDOl'9 

IPDIO. 1// PLOTOU20 

PL0T0021 
PLCTC’022 
PLOT0023 
PLGTC024 
PLOTOC 2 5 
PLOT0026 
PLCT')r.27 
PL0T0v28 
PLCT0029 
PL0T0C30 
PL0TD031 
PLOTOC' 3 2 
PL0T0033 
PL0T0034 
PLOT0O35 
PL0T0036 


PLOT ex (PLOT, NT, NUMBXrM ARK, 1 WRITE ) 


C5 



LINE {62> = 2 
UNE (67) = 3 
LINE (7?) = 4 
LINE (77) = 5 
LINE (B?) = 6 
LINE (87) = 7 
LINE (9?) = 8 
LINE 197) = 9 
LINE (1025= 1 
LINE 1103 >= 0 
WRITE {1WRITE.U70) LINE 

100 FORMAT (20X,2U,4A1. II , 4Al , I 1 , 4A1. , II t 4A 1 » I 1 , 4A 1 , 1 1 , 4A1 . 1 1 ,4A1 , 

L n.4Al, I 1,4A1,I1,4A1 ,n.4Al,Il,4Al, U,4A1,II,4A1.I 1,4A1, 11,4A1, 
21 1,4A1, Il,4Alt Ilf 4A1, I 1 ,-iAl ,211) 

00 3 1=1.103 
3 LINE! I )=BLANK 
LINE(1)=D0T 
. LINFI521=D0T 
HNE(I03)=DnT 
DO 13 I = 1,MIMBR 
J=SPACE+PLnT ( : )+52. 5 
LINE! .1! =MARK 

WRITE (IWRITEflOl) NTIDfLINE 
LINE! J)= BLANK 
LINE( 52)=OOT 
13 CONTINUE 

101' F0RMAT(9X, 110,iX,103Al) 

RETURN 

END 


PLOTO037 
PL0TCI0 38 
PL0T0039 
PLCT0040 
PLQT0041 
PLCT0042 
PLGT0043 
PL0T0044 
PL0T0C45 
PLOT0046 
PI DT0O47 
PL0T0048 
PLOT0049 
PL0T0050 
PL0T0051 
PLOTOC52 
PL0TO053 
PL0T0054 
PL0T0055 
PL0T0056 
PLCT0057 
PLOT0058 
PLCT0059 
PLOTO060 
PL0T0061 
PLOT0062 
PL0T0063 
PLOT0064 
PL0T0065 



SURROUTINJ: MAT^LT t A , N1 , N2 , 9 , HI ♦ M2 , C t I WR I TM 
IMPLICIT RfeALi-BIA-C.G-Z ) 

DIMENSION A(3.91, 8(3-3), C(3,3) 

C 

IF (N2- Ml) 10,2;), 10 

10 WRITE (IWRITE,!) Nl, N2, Ml, M2, 

I ( ( A( I, Jl ,J=1,N21 , 1=1, Nl) , ( (Q( I, J) ,J = 1,M? ) , 1 = 1 ,M1) 

1 FORMAT (IHl 26H MATRIX MULTIPLY ROUTINE // 

1 25H INCOMPATIBILITY ERROR A( , 13, IH, 15, IH) , 3X 

2 3H B( , 15 , IH, 15, lU ) n 18H A MATRIX OY ROWS / 

3 18ri R MATRIX BY ROWS // (6D20o8) ) 

• C 

20 DO 30 1=1, Nl 
DO 30 J=1,M2 
Cl I,J> = o.n 
on 40 K=1,N2 

40 C( I, J) = C( I ,J) + A( I,KI=;'B(K,J) 

30 CONTINUE 
RETURN 
END 


MATMOCOl 
MATM0002 
MATMC0C3 
MATM0004 
MATHC;>05 
HATM0006 
MATM0007 
MATM4CC8 
MATMJOCO 
MATMOOlO 
MAT MOO 11 
MATM0012 
MATMnOX3 
MATMOOIA 
HATM0O15 
MATM0016 
MAT MOO 17 
MATM0t'18 
MATMOOlO 
MATM0020 


o 



05 

CO 


SUBROUTINE f1ATXHA,AT» 
IMPLICIT REALM'S! A-G. n-Z > 
niMENSION A(3,3) » AT(3»3) 
nn 1 1=1,3 
on I J=l,3 
AT(i,j)=Atj. n 
1 CONTINUE 
RETURN 
END 


MATXOOCl 

MA rxooo2 

HATXU003 

MATX0004 

MATX0005 

,MATX0006 

NATX0007 

MATX0008 

MATXOOO? 



SUBROUTINE V ECMLT ( A » N1 1 N2 , B . C ) 
IMPLICIT REAL'-8 ( A-G.O-Z ) 
DIMENSION A( 4,4) .31 A-) ,C( A) 

no 10 1=1 ,N1 
C(i)=o.n 
no 10 j=i,N2 

IP C( I ) = CI I 1 + All . J) 

RETURN 

END 


TO 

<o 


V ECHO 00 
VECMOOO 
VECHOCO 
VECMOOO 
VECMOUO 
VECMOOO 
VECMOOJ 
VECMO'j ) 
VECMOG j 



SUBROUTINE ORTHOCBl 
IMPLICIT RFAL#6(A-G,D-2) 

DIMENSION 813, ,Ct 3.3) 

SORT(X)=OSORT(XI 

C(1 ,U=B(2,2)<'B(3;3)-B{3,2)»i8(2,3) 
C(1.2)=8(3,l )=>!R{2,3)-B(2,1)*B(3,3) 
Ca,3) = B<2,l)*B(3,2)-B(2,2)«-B( 3,1) 
C(3a)=BU,2)»B(2,3l-3(2,2)*B(l,3) 
r.(3,2) = B(2,l )-i=B( 1,3)-B<1, l)>f'B( 2,31 
C(3,3) = R(l,l)*B<2,2)-8{l,2)».B{2(.l) 
C(2.l)--B(3,?)vB( l,3)-na,2)-i'R(3,3) 
C<2,2)=B(1,1)*B(3,3)-3<3,1)+B(1,3) 
C(2,3)=B(3,1)^58^1,2>-3(3,2)>^B(1,1) 

DO 1 1=1,3 
on 1 J=l,3 

1 B(I ,J)=Bl 5,J)+C( I,J) 
on 2 1=1,3 

D=SORT(B(Z ,1)=!'’«2+Bf 1,2 )^*2 + Bn,3!*'P2) 

DO 2 J=l,3 

B( I ,J> = B{ I.J )/D 

2 CONTINUE 
RETURN 
END 


CRTHOOOl 

ORTHO 002 

ORTH0003 

ORTHOOOA 

0RTH00D5 

ORTH0006 

ORTHOfiOT 

0RTH0008 

0RTH0009 

ORTHOOIO. 

ORTHOOll 

0RTH0f112 

ORTH0013 

□RTH0014 

0RTH0015 

ORTH0016 

ORTH0017 

GRTH0018 

ORTH0019 

ORTH0020 

0RTH0021 

OR TH0022 

ORTHO023 



SURROUTINE TRUTH (T,RT) 

C SINGIF AXIS COiNSTANT-RATE 

IMPLICIT real:' ?( A-G.O-Z ) 
or MENS I ON PT(T,3)^ At 5). TM ( 6 I ' 

SINt X)=DSIM X ) 

Cr)S(Xl = OCOS! X) 

SORT<X>=OSURT(X) 

PI=3. lAl 5T2653589732 

OGTORO = PI/lRO.n 

THETAN = '1,0 

At n = 10.0 

At 2) = ■'•.O 

At 3) = C-.O 

AtA) = n.o 

At5> = O.p 

TMt 1) = T 

no 7 1=1, 5 

XI = 1 

thetan = tAt n/xn TMtn + theian 
TM tI + ll = TMtns=TM(l) 

7 CONTINUE 
. 00 lO 1=1,3 

no 10 j=i,3 
BTtI,J) = 0,0 
10 . CONTINUE 

THETAN = THETANi-DRTOEO 
SI = SINtTHETAN) 

CT = CCStTHETAN) 

RTt2,2) = 1.0 
BTtltl) = CT 
BTt3,l) = -ST 
BT(3,3) = BTtl.l) 

8Tt 1,31 = -BT(3.1 1 

RETURN 

END 


TRUTJCn 
TftUT‘)00 2 
TRUT6003 
TRUTOOOA 
TRUTOt C5 
TRUTOf‘06 
TRUT0007 
TRUTji'OS 
TRUTO-J09 
TRUTCliJln 
TRUTCOll 
TRUT1012 
TRUT0013 
TRUTOCIA 
TRUT0015 
TRUTC016 
TRUT0317 
TRUT0018 
TRUT0C19 
TRUT0020 
TRUT'j 021 
TRUT0022 
TRUT0023 
TRUTn02A 
TRUT0025 
TRUT0026 
TRUT0(;27 
TRUT0O28 
TRUTfJ'.'29 
TRUTC'03'J 
TRUTi-OSl 
TPUT0032 
TRUT0033 
TRUT003A 
TRUT0035 



SUBROUTINE PULSE ! T ,T I NT ,DTHET A 

PULSOnol 

IMPLICIT REAL’<8( A-R,0-Z ) 

PULS0002 

•GENERATES PULSES FOR THE POLTNOMIAL INPUT CASE — SINGLE AXIS 

PULS00O3 

DIMENSION DTHETAO) ,AIA) 

PULS0004 

P1=3„1415<5265 3 58973E 

PULS0005 

OGTORD = PI/180,0 

PULS0C06 

A0=10-0 

PULS0007 

A{ 1 ) = 0,0 

PULS0008 

A( 2> =0,0 

PULS0009 

A(3) = 0,0 

PULSOOJO 

AfA) - 0.0 

PULSOOll 

DTHETAI 1 » = 0,0 

PULS0012 

OTHETAO ) = 0.0 

PULS0013 

DTI = TINT 

PULS0014 

DT2 = TINT*>^2 

PULS0015 

DT3 = TINT**3 

PULS0016 

DT4 = TINT^'frA 

PULS0017 

n = T 

PULSOOIS 

T2 = T^'ap 

PULS0019 

T3 = T>!'=^3 

PULSOO20 

TA = 7>'.*4 

PULS0021 

PARTI = AO-^Ad J#lTl+0-5*DTl) 

PULSQ022 

PART2 = Al21*IT2'fTl>!<DTl-!-DT2/3,r;> 

PULS0023 

PARTS = AOJ'^I T3 + 1.5*T2W0T1 + T1'.''DT2 + .25'‘DT3) 

PULS0024 

PART4 = A(4|-MT4-*-2.O + T3=«'OTl + 2.O"«T2'i‘DT2-l-Tl'J’DT3'+,2'i<0T4) 

PULS0025 

0THETA<2> = (PARTUPART2'+PART3-t-PART4>ADTl + DGTORO 

PULS0026 

RETURN 

, PULS0027 

FNO 

PULS0O28. 



CO 


SUBROUTINE ALGOR I DTHETA , BC , I WR ITE ) 

C. DIRECTION COSINE MATRIX, FOURTH ORDER 
IMPLICIT REALt8( A-G,0-2) 

COMMON riNT.NCNT 
COMMON /ABC/ PLOT.PPKINT 

DIMENSION DTHETA<3),BC(3,3),UNITY(3.3),DTHET1(3) 

DIMENSION DTHET2(3) ,THMAT1(3, 3S ,THMAT2(3,31,TEMP1(3,3) ,OMG2(3,3) 
DIMENSION CMGO(3,3»,TEMP2<3,3) 

DIMENSION OMG3I 3 ,3 ) . BN ! 3 , 3 I , CN ( 3 , 3 I , DN ( 3 , 3 ) 

SIN(X» = DSIMX) 

SORTIX)=DSORT(X» 
cost X» = DCOS( X» 

DT = TINT/2,0 
GO TO (100,200), NCNT 
100 T= 0.0 

NCNT = 2 

2CC) CALI. PULSE (T.DT.DTHETU 
T = T + TINT 

DO 300 1=1,3 ' ■ 

3^10 DTHET2(l) = DTHETA( II - DTHETl(I) 

DO 305 1=1.3 
THMATK 1,1) = (i.O 
THMAT2U ,I ) = 0,0 

305 CONTINUE 

THMATl 11,2) = -DTHETK 3) 

THMATK 2, 3) = -DTHETK 1) 

THMATK3,!) = -DTHETK2) 

THMATK1,3) = DTHETK2) 

THMATK2t1) = DTHETK3) 

THMATK 3,2) = DTHSTKl) 

THMAT2(1,2) = -0THET2(3) 

THMAT2(2.3) = -DTHET2(1) • 

THHAT2I3,1) = -0THET2(2) 

THMAT?( 1,3) = 0THET2(2) 

THMAT?(2,i) = DTHET2I3) 

THMAT2(3,2> = 0THET2(1) 


ALGOOOOl 
ALG00002 
ALG00O03 
ALG00004 
ALGDvIC-05 
ALG0a0O6 
ALG00007 
ALGOOOG8 
ALG0U(H,9 
ALGOCCIO 
ALGOOOll 
ALG00012 
ALGOL‘013 
ALG000I4 
ALGJ0015 
AL GOOD 16 
ALG00017 
ALG00018 
ALG00019 
A L GOOD 20 
ALG00021 
ALG00022 
ALC00023 
ALG0002A 
ALG00025 
ALG00026 
ALG00027 
ALG00028 
ALG00029 
A L GOG 030 
.,LG00031 
ALG00032 
ALG00033 
ALG00D34 
ALG00035 
ALG00036 



on 310 1=1,3 

DO 310 J=l,3 

GMGOd.J) = (3,0««THMATl(I,J)-THMAT2n,J} )/Ti'nT 
nMG?n,J) = (3*0’>=THMAT2(I,«ji)--THMAJU .i,0) l/TINT 
nHG3(I ,J> = (THMATl ( I,J )-!-THHAT2 { I , o > ) /T INT 
310 CONTINUE 

CALL HA7HL.r{8C,3,3;0MG0,3,3, r£MPl, IHP. ife'j 
00 500 I=i,3 
on 500 J=l,3 

TEMPI n,J}=TEKPlUoJJ»',TIMt ‘ 

500 TEMP2n..rf = r£MPl{ I,J)/2,0 + 5C(1',JJ ■ 

*' CALL MATHLT.i'TENP2,3.3,0^<G3,3,3,BN, IHRITE) 

. ' no 501 1 = 1 . 3 . 

DO 501 J=l,3 • 

8N< I,J)=BN(I,J)>f^TINT' 

‘501 TEMP2H , J)=BNi I,.n /2.0 + BC,M,J> 

CALL MATMLTITEMP2,3,3.,OMG3t3,3,CN,IHRITE) 

DO 502 Ir=l,3 l' . I ‘ , 

DO' 50.2 J = l,,3 ' . 

. CN( I , J) = CNCI . J Il'TINT 
502 TEMP2(I,J(=CN( I.J) BC(UJ) 

CALL. MA-T.HL Tl TEMP2 . 3 , 3 , 0MG2’, 3 - 3 , O.N , I WR I TE ) 

00.503 1=1,3 
Ort 503 J=l,3 
0N( I. j)=ON{ I, J ) hTINT 

5b3--BCn,J)-=BC<I,J) + TEMPI! I, J »/5,0 BN ( I , J ) / 3 o 0. CN(I,J)/3„0 

1 ON! [-JI/6.0 
■ IFlt.GE.PLOT) GO TO 888 
IFt,r.GE.PPRINT) GO TO 888 
• RETURN 

,888 CALL ORTnn'(BC) 

RETURN 

END 


ALG00037 

ALG00G33 

ALG00039 

ALG00040 

ALG00041 

ALG00042 

ALG00043 

ALG00044 

ALG00045 

AL600046 

ALG00047 

ALG00048 

ALG00049 

ALGD0050 

ALG00051 

ALGOOO‘52 

aLG0QO53 

AL600054 

ALG00055 

ALG00056 

ALGCJ0057 

ALG00058 

ALGC0059 

ALG0D060 

ALG00061 

ALG00062 

ALG00063 

ALG00064 
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